﻿#pragma once
#include "stm32f4xx.h"
//#include "arm_math.h"
#include "matrix.h"


#define DIV_180_PI       (float)(57.295779513f)       // 乘DIV_180_PI弧度转角度 除DIV_180_PI角度转弧度
#define GRAVITY          (float)(9.8f)                // g转m/s²


typedef struct {
	float period;                       // 姿态更新周期
	float half_T;                       // 姿态更新周期的1/2      单位：s

	float I_matrix_data[16];                 // 单位矩阵              4 * 4
	matrix_instance_f32 I_matrix;
	float Q_Val = 0.01;
	float Q_matrix_data[16];                 // 过程噪声协方差矩阵      4 * 4
	matrix_instance_f32 Q_matrix;

	float R_Val = 100000;
	float R_matrix_data[9];                  // 观测噪声协方差矩阵      3 * 3
	matrix_instance_f32 R_matrix;

	float A_data[16];                        // 状态转移矩阵           4 * 4
	matrix_instance_f32 A;
	float H_data[12];                        // 观测矩阵              3 * 4
	matrix_instance_f32 H;

	float HT_data[12];
	matrix_instance_f32 HT;

	float P_Val = 100000;
	float p_hat_data[16];                    // 误差协方差矩阵         4 * 4
	matrix_instance_f32 p_hat;

	float K_data[12];                        // 卡尔曼增益矩阵         4 * 3
	matrix_instance_f32 K;

	float Kq_data[4];                        // 四元数卡尔曼增益        4 * 1
	matrix_instance_f32 Kq;

	float T_data[3];                         // 残差向量               3 * 1
	matrix_instance_f32 T;
	float q_data[4];                         // 机体四元数             4 * 1
	matrix_instance_f32 q;
	float q_k_data[4];                       // 优化后的四元数          4 * 1
	matrix_instance_f32 q_k;




	float temp_matrix1_data[16];
	matrix_instance_f32 temp_matrix1;
	float temp_matrix2_data[16];
	matrix_instance_f32 temp_matrix2;
	float temp_matrix3_data[16];
	matrix_instance_f32 temp_matrix3;


	float temp_matrix4_data[12];
	matrix_instance_f32 temp_matrix4;
	float temp_matrix5_data[12];
	matrix_instance_f32 temp_matrix5;

	float temp_matrix6_data[9];
	matrix_instance_f32 temp_matrix6;

	float temp_matrix7_data[9];
	matrix_instance_f32 temp_matrix7;

	float temp_matrix8_data[9];
	matrix_instance_f32 temp_matrix8;

	float temp_matrix9_data[16];
	matrix_instance_f32 temp_matrix9;
	float temp_matrix10_data[16];
	matrix_instance_f32 temp_matrix10;
	float temp_matrix11_data[16];
	matrix_instance_f32 temp_matrix11;
	float vector_hat[3];                // 观测重力加速度          3 * 1
	float roll;
	float pitch;
	float yaw;
	void (*filter)(float, float, float,   // EKF更新姿态函数指针
		float, float, float);
}EKF_StructTypeDef;




void EKF_init(void);

void normalize_quternion(float* qt);




void ekf_update(float gx, float gy, float gz, float ax, float ay, float az);

void quternion2euler();


//EKF_StructTypeDef imu_ekf;







//#pragma once
//#include "stm32f4xx.h"
//#include "matrix__.h"
//
//#include "arm_math.h"
//#define DIV_180_PI       (float)(57.295779513f)       // 乘DIV_180_PI弧度转角度 除DIV_180_PI角度转弧度
//#define GRAVITY          (float)(9.8f)                // g转m/s²
//
//
//typedef struct {
//	float period;                       // 姿态更新周期
//	float half_T;                       // 姿态更新周期的1/2      单位：s
//
//	float I_matrix[16];                 // 单位矩阵              4 * 4
//	float Q_matrix[16];                 // 过程噪声协方差矩阵      4 * 4
//	float R_matrix[9];                  // 观测噪声协方差矩阵      3 * 3
//	float A[16];                        // 状态转移矩阵           4 * 4
//	float H[12];                        // 观测矩阵              3 * 4
//	float p_hat[16];                    // 误差协方差矩阵         4 * 4
//	float K[12];                        // 卡尔曼增益矩阵         4 * 3
//	float Kq[4];                        // 四元数卡尔曼增益        4 * 1
//	float T[3];                         // 残差向量               3 * 1
//	float q[4];                         // 机体四元数             4 * 1
//	float q_k[4];                       // 优化后的四元数          4 * 1
//
//	float vector_hat[3];                // 观测重力加速度          3 * 1
//	float roll;
//	float pitch;
//	float yaw;
//	void (*filter)(float, float, float,   // EKF更新姿态函数指针
//		float, float, float);
//}EKF_StructTypeDef;
//
//
//
//
//void EKF_init(void);
//
//void normalize_quternion(float* qt);
//
//
//
//
//void ekf_update(float gx, float gy, float gz, float ax, float ay, float az);
//
//void quternion2euler();